#include "Chassis.h"

void Chassis::Init()
{
    // 电机初始化
    M_LF->Init();
    M_LB->Init();
    M_RF->Init();
    M_RB->Init();

    // 闭环控制参数初始化
    M_LF->speedLoop.SetPIDParam(20.0f, 10.0f, 0, 5000.0f, 30000.0f);
    M_LF->currentLoop.SetPIDParam(0, 0, 0, 0, 0);
    M_LB->speedLoop.SetPIDParam(20.0f, 10.0f, 0, 5000.0f, 30000.0f);
    M_LB->currentLoop.SetPIDParam(0, 0, 0, 0, 0);
    M_RF->speedLoop.SetPIDParam(20.0f, 10.0f, 0, 5000.0f, 30000.0f);
    M_RF->currentLoop.SetPIDParam(0, 0, 0, 0, 0);
    M_RB->speedLoop.SetPIDParam(20.0f, 10.0f, 0, 5000.0f, 30000.0f);
    M_RB->currentLoop.SetPIDParam(0, 0, 0, 0, 0);

    yawAngleLoop.SetPIDParam(45.0f, 0, 0, 0, 1000.0f);//8
    yawSpeedLoop.SetPIDParam(1.0f, 0, 0, 0, 1000.0f);//1

    // 陀螺仪初始化
    icm->ConfigInit(inv_io_hal_read_reg, inv_io_hal_write_reg, 1024 * 32, 1024 * 32);
    rc = icm->SetupInvDevice(&icm->icm_serif, true);
    icm->RunSelfTest();
    rc = icm->InitInvAGMBiases();
    rc |= icm->InitInvAGMAlgo();
    rc = icm->ConfigureInvDevice((uint8_t)IS_LOW_NOISE_MODE, 2,
                                 2000, ICM426XX_ACCEL_CONFIG0_ODR_1_KHZ,
                                 ICM426XX_GYRO_CONFIG0_ODR_1_KHZ, (uint8_t)USE_CLK_IN);
}

void Chassis::Update(float *_x_target, float *_y_target, float *_yawAngle)
{
    x_target = *_x_target;
    y_target = *_y_target;
    yawAngle = *_yawAngle;
}

void Chassis::Adjust()
{
     if (irq_from_device & TO_MASK(1))
    {
        // rc = icm.GetDataFromInvDevice();
        rc = icm->GetDataFromInvDeviceFIFO();
        icm->fixedpoint_to_float(Icm42688::output.grv_quat_q30, rvquat, 30, 4);
        icm->quaternions_to_angles(rvquat);
        inv_disable_irq();
        irq_from_device &= ~TO_MASK(1);
        inv_enable_irq();
    }

    yawAngleLoop.Target = d_yawTarget;
    yawAngleLoop.Current = icm->eulerAngle.yaw;
    //yawSpeedLoop.Target = yawAngleLoop.Adjust();
    //yawSpeedLoop.Current = -Icm42688::input.sRgyr_data[2]/icm->get_gyr_sensitivity();
    r_target = yawAngleLoop.Adjust();

    resolve.Update(x_target, y_target, -r_target);
    resolve.MC_SpeedCalc();
    M_LF->Update(resolve.getWheelTarget()[LF]);
    M_LB->Update(resolve.getWheelTarget()[LB]);
    M_RB->Update(resolve.getWheelTarget()[RB]);
    M_RF->Update(resolve.getWheelTarget()[RF]);

    M_LF->Adjust();
    M_LB->Adjust();
    M_RF->Adjust();
    M_RB->Adjust();
}

void Chassis::Acuate()
{
    M_LF->Acuate();
    M_LB->Acuate();
    M_RF->Acuate();
    M_RB->Acuate();
}
